#include "MatrixUtils.h"

#include <cassert>
#include <cfloat>
#include <cmath>
#include <cstring>
#include <fstream>
#include <iostream>
#include <sstream>

namespace Utils
{
    Matriz::Matriz(const uint32_t rows, const uint32_t columns)
    	: mData(new double[rows * columns])
        , mRows(rows)
    	, mColumns(columns)
    {
        assert(mData && "Matriz::Matriz: NULL pointer");
    }

    Matriz::Matriz(const Matriz& matriz)
    	: mData(new double[matriz.mRows * matriz.mColumns])
        , mRows(matriz.mRows)
    	, mColumns(matriz.mColumns)
    {
    	memcpy(mData, matriz.mData, matriz.mRows * matriz.mColumns * sizeof(double));
    }

    Matriz::~Matriz()
    {
    	delete[] mData;
    }

    Matriz& Matriz::operator=(const Matriz& matriz)
    {
        if(this != &matriz)
        {
            assert(mRows == matriz.mRows && "operator= Matrices have different number of rows");
        	assert(mColumns == matriz.mColumns && "operator=: Matrices have different number of columns");
        	assert(mData && "operator=: NULL pointer");
        	assert(matriz.mData && "operator=: NULL pointer");

        	const uint32_t dataSize = mRows * mColumns * sizeof(double);
        	memcpy(mData, matriz.mData, dataSize);
        } 
    	
    	return *this;
    }

    void PrintMatrix(const Matriz& matriz)
    {
    	assert(matriz.mData && "PrintMatrix: NULL pointer");

    	const uint32_t numberOfElements = matriz.mRows * matriz.mColumns;
    	for(uint32_t i = 0; i < numberOfElements; ++i)
    	{
    		std::cout << "Matrix[" << i << "| = " << matriz.mData[i] << std::endl;
    	}
    }

    Matriz AddMatrices(const Matriz& a, const Matriz& b)
    {
    	assert(a.mData && "AddMatrices: NULL pointer");
    	assert(b.mData && "AddMatrices: NULL pointer");
    	assert(a.mRows == b.mRows && "AddMatrices: Matrices have different number of rows");
    	assert(a.mColumns == b.mColumns && "AddMatrices: Matrices have different number of columns");

    	// Copy the first matrix
    	const uint32_t dataSize = a.mRows * a.mColumns;
    	Matriz result(a.mRows, a.mColumns);
    	memcpy(result.mData, a.mData, dataSize * sizeof(double));

    	// Add the second matrix.
    	const double *source = b.mData;
    	double *destination = result.mData;
    	for(uint32_t i = 0; i < dataSize; ++i)
    	{
    		*destination += *source;
    		++source;
    		++destination;
    	}

    	return result;
    }

    Matriz SubtractMatrices(const Matriz& a, const Matriz& b)
    {
    	assert(a.mData && "SubtractMatrices: NULL pointer");
    	assert(b.mData && "SubtractMatrices: NULL pointer");
    	assert(a.mRows == b.mRows && "SubtractMatrices: Matrices have different number of rows");
    	assert(a.mColumns == b.mColumns && "SubtractMatrices: Matrices have different number of columns");

    	// Copy the first matrix
    	const uint32_t dataSize = a.mRows * a.mColumns;
    	Matriz result(a.mRows, a.mColumns);
    	memcpy(result.mData, a.mData, dataSize * sizeof(double));

    	// Subtract the second matrix.
    	const double *source = b.mData;
    	double *destination = result.mData;
    	for(uint32_t i = 0; i < dataSize; ++i)
    	{
    		*destination -= *source;
    		++source;
    		++destination;
    	}

    	return result;
    }

    Matriz MultiplyMatrices(const Matriz& a, const Matriz& b)
    {
    	assert(a.mColumns == b.mRows && "MultiplyMatrices: middle rows and columns sizes are different");

    	// Init the result matrix.
    	Matriz result(a.mRows, b.mColumns);
    	const uint32_t numberOfElements = a.mRows * b.mColumns;
    	memset(result.mData, 0, numberOfElements * sizeof(double));
    	
    	double *resultData = result.mData;
    	const double *aData = a.mData;
    	const uint32_t bElements = b.mRows * b.mColumns;
    	const double * const resultLastPosition = resultData + numberOfElements;
    	const double * const bLastPosition = b.mData + bElements;
    	
    	for(double *resultData = result.mData; resultData != resultLastPosition; resultData += result.mColumns)
    	{
    		for(const double *bData = b.mData; bData != bLastPosition; bData += b.mColumns)
    		{
    			for(uint32_t i = 0; i < result.mColumns; ++i)
    			{
    				const double value = *aData * bData[i];
    				resultData[i] += value;
    			}
    			++aData;
    		}
    	}
    	
    	return result;
    }

    Matriz MultiplyMatrixByLiteral(const Matriz& matrix, const double lit)
    {
    	assert(matrix.mData && "MultiplyMatrixByLiteral: NULL pointer");

    	Matriz result(matrix.mRows, matrix.mColumns);
    	const uint32_t numberOfElements = matrix.mRows * matrix.mColumns;
    	memcpy(result.mData, matrix.mData, numberOfElements * sizeof(double));
    	for(uint32_t i = 0; i < matrix.mRows * matrix.mColumns; ++i)
    	{
    		result.mData[i] *= lit;
    	}

    	return result;
    }

    // Build matrix from a file specifying the file
    // path with the numbers to build the matrix, the number
    // of rows and the numbers of columns.
    Matriz GetMatrixFromFile(const std::string& filePath, const uint32_t rows, const uint32_t columns)
    {
    	// Open the file for read.
    	std::ifstream file (filePath.c_str());

    	assert(file && "GetMatrixFromFile:Failed to open file.");

    	// Create the columns.
    	Matriz matrix(rows, columns);
    	
    	// Fill each row with the numbers from the file.
    	double *matrixData = matrix.mData;
    	for(uint32_t i = 0; i < rows; ++i)
    	{
    		// Extract the i-line of the file.
    		char buffer[100000];
    		file.getline(buffer, 100000);
    		//std::istringstream ss(buffer, 100000);
    		std::istringstream ss(buffer, std::istringstream::in);
    		
    		// Store the first row.
    		for(uint32_t j = 0; j < columns; ++j)
    		{
    			double d = 0.0f;
    			ss >> d;
    			
    			*matrixData = d;
    			++matrixData;
    		}

    	}

    	// Close the file.
    	file.close();

    	return matrix;
    }


    // Returns the absolute value of the matrix.
    Matriz GetAbsoluteMatrix(const Matriz& matrix)
    {
    	// Build a new matrix.
    	Matriz newMatrix(matrix.mRows, matrix.mColumns);
    	
    	// Iterate through the matrix and compute the absolute value
    	// of each member.
    	for(uint32_t i = 0; i < matrix.mRows * matrix.mColumns; ++i)
    	{
    		newMatrix.mData[i] = (0.0 <= matrix.mData[i]) ? matrix.mData[i] : -matrix.mData[i];
    	}

    	return newMatrix;
    }


    // Get the max element of the matrix
    double GetMaxElement(const Matriz& matrix)
    {
    	assert(matrix.mData && "GetMaxElement: The matrix must have at least one element.");

    	double currentMaxElement = DBL_MIN;
    	
    	// Iterate through the matrix, searching for the max element.
    	for(uint32_t i = 0; i < matrix.mRows * matrix.mColumns; ++i)
    	{
    		const double value = matrix.mData[i]; 
    		if(currentMaxElement < value)
    			currentMaxElement = value;
    	}

    	return currentMaxElement;
    }


    // Get the norm of a vector matrix (nx1)
    double GetInfiniteVectorNorm(const Matriz& vector)
    {
    	// Check that the matrix is a vector (i.e. (nx1))
    	bool isAVector = (vector.mColumns > 0 && vector.mRows == 1) || (vector.mColumns == 1 && vector.mRows > 0);
    	assert(isAVector && "GetVectorNorm: The matrix is not a vector.");

    	// Get the absolute vector.
    	Matriz absoluteVector = GetAbsoluteMatrix(vector);

    	// Return the max element of the absolute vector.
    	return GetMaxElement(absoluteVector);	
    }


    // Get the first order norm of a vector matrix (nx1)
    double GetFirstOrderVectorNorm(const Matriz& vector)
    {
    	// Check that the matrix is a vector (i.e. (nx1))
    	bool isAVector = (vector.mColumns > 0 && vector.mRows == 1) || (vector.mColumns == 1 && vector.mRows > 0);
    	assert(isAVector && "GetVectorNorm: The matrix is not a vector.");

    	// Compute the sum of all the absolute elements of the vector.
    	double sum = 0.0;

    	for(uint32_t i = 0; i < vector.mRows * vector.mColumns; ++i)
    		sum += (0.0 <= vector.mData[i]) ? vector.mData[i] : -vector.mData[i];

    	return sum;
    }


    // Add all the elements of a matrix and returns it.
    double AddMatrixElements(const Matriz& matrix)
    {
    	assert(matrix.mRows > 0 && matrix.mColumns > 0 && "AddMatrixElements: The matrix must have elements");
    	
    	// Iterate through the matrix, adding each element.
    	double sum = 0.0;
    	
    	for(uint32_t i = 0; i < matrix.mRows * matrix.mColumns; ++i)
    	{
    		sum += matrix.mData[i];
    	}

    	return sum;
    }

    // Write the pairs to a file
    void WriteMatrixToFile(const std::string& filePath, const Matriz& matrix)
    {
    	// Open file.
    	std::ofstream file;
    	file.open(filePath.c_str());

    	// Check that file was correctly opened.
    	assert(file && "WriteMatrixToFile: Error in file opening.");

    	// Write matrix to file.
    	for(uint32_t i = 0; i < matrix.mRows; ++i)
    	{
    		const uint32_t newOffset = i * matrix.mColumns;
    		for(uint32_t j = 0; j < matrix.mColumns; ++j)
    		{
    			file << matrix.mData[newOffset + j] << " ";
    		}
    		file << std::endl;
    	}

    	// Close file.
    	file.close();
    }
}
